public class PID {
	protected double kP;
	protected double kI;
	protected double kD;
	protected double error;
	protected double errorPrevious;
	protected double errorIntegral;
	protected double errorDifferential;
	
	public static double _MIN = -100;
	public static double _MAX = 100;
	
	public PID(double P, double I, double D) {
		this.kP = P;
		this.kI = I;
		this.kD = D;
		this.error = 0;
		this.errorPrevious = 0;
		this.errorIntegral = 0;
		this.errorDifferential = 0;
	}
	
	public double Get(double balancedValue, int currentValue) {
		// This is the value we will return
		double pidValue;
		
		// Subtract center value
		this.error = currentValue - balancedValue;
		// Remove variation if far from surface
		if (this.error <= 0)
		  this.error *= 16 / 10;

		// Compute differential error
		this.errorDifferential = this.error - this.errorPrevious;
		this.errorPrevious = this.error;

		// Compute integral error (w/fast damping)
		this.errorIntegral = (this.errorIntegral + this.error) * 2 / 3;

		// Add Differential, Proportional, and Integral Components
		pidValue = this.kD * this.errorDifferential + this.kP * this.error + this.kI * this.errorIntegral;
		// Scale PID value
		pidValue /= 45;

		// Keep between MIN and MAX
		if (pidValue < PID._MIN)
		  pidValue = PID._MIN;
		else if (pidValue > PID._MAX)
		  pidValue = PID._MAX;

		// Done!
		return pidValue;
	}
}